//pid控制 还不能在线调参 plane_pid.cpp可以
#include <rclcpp/rclcpp.hpp>

// MoveIt Servo 
#include <moveit_servo/servo_parameters.h>
#include <moveit_servo/servo.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>

#include <control_msgs/msg/joint_jog.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <tf2_ros/buffer.h>

#include <geometry_msgs/msg/wrench_stamped.hpp>  // 新增：订阅 /sim_force
#include <atomic>                                 // ★ 新增：原子变量用于跨线程读写Fz

using namespace std::chrono_literals;

static const rclcpp::Logger LOGGER =
    rclcpp::get_logger("smc0.servo_node.cpp");

// 先声明将用于向 Servo 发布指令的节点与发布器指针
rclcpp::Node::SharedPtr node_;
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_cmd_pub_;
rclcpp::Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr sim_force_sub_;

// ===========================
// ★ 新增：力控PID相关参数与状态
// ===========================
constexpr double kFzTarget = 5.0;     // 目标恒力：5N（Z轴）
constexpr double kKp = 0.05;          // 比例增益（先小再逐步加大）
constexpr double kKi = 0.00;          // 积分增益（缺省0，调稳定后再开启）
constexpr double kKd = 0.00;         // 微分增益（小量抑制抖动）
constexpr double kMaxVel = 0.5;      // X向最大速度（m/s），避免过激
constexpr double kMaxI = 2.0;         // 积分项限幅，防积分饱和

std::atomic<double> g_latest_fz{0.0}; // 最近接收到的Fz（N）
double g_int_acc = 0.0;               // 积分累积
double g_prev_err = 0.0;              // 上次误差
rclcpp::Time g_prev_time;             // 上次控制时间戳
bool g_has_prev_time = false;

// ============
// 订阅回调：仅更新Fz
// ============
void simForceCallback(const geometry_msgs::msg::WrenchStamped::SharedPtr msg)
{
  g_latest_fz.store(msg->wrench.force.z, std::memory_order_relaxed);

  // 可选：节流打印查看实时力
  RCLCPP_INFO_THROTTLE(LOGGER, *node_->get_clock(), 500,
                       "[/sim_force] Fz=%.3f N (target=%.2f)",
                       msg->wrench.force.z, kFzTarget);
}

// ============
// 发布控制指令（PID）
// ============
void publishCommands()
{
  // 计算dt
  const rclcpp::Time now = node_->now();
  double dt = 0.05;  // 默认50ms
  if (g_has_prev_time) {
    dt = std::max(1e-3, (now - g_prev_time).seconds()); // 下限1ms，避免除零
  }
  g_prev_time = now;
  g_has_prev_time = true;

  // 读当前测得的Fz
  const double fz = g_latest_fz.load(std::memory_order_relaxed);
  const double err = kFzTarget - fz;  // <5N -> 正误差 -> +x前进；>5N -> 负误差 -> -x后退

  // PID
  g_int_acc += err * dt;
  // 积分限幅
  if (g_int_acc > kMaxI)  g_int_acc = kMaxI;
  if (g_int_acc < -kMaxI) g_int_acc = -kMaxI;

  const double derr = (err - g_prev_err) / dt;
  g_prev_err = err;

  double ux = kKp * err + kKi * g_int_acc + kKd * derr;

  // 速度限幅（m/s）
  if (ux >  kMaxVel) ux =  kMaxVel;
  if (ux < -kMaxVel) ux = -kMaxVel;

  // 组包并发布
  auto msg = std::make_unique<geometry_msgs::msg::TwistStamped>();
  msg->header.stamp = now;
  msg->header.frame_id = "base_link";

  msg->twist.linear.x = ux;   
  msg->twist.linear.y = 0.0;
  msg->twist.linear.z = 0.0;
  msg->twist.angular.x = 0.0;
  msg->twist.angular.y = 0.0;
  msg->twist.angular.z = 0.0;

  twist_cmd_pub_->publish(std::move(msg));

  // 节流打印：看误差与输出
  RCLCPP_INFO_THROTTLE(LOGGER, *node_->get_clock(), 500,
                       "[PID] Fz=%.3fN err=%.3f u_x=%.3fm/s (dt=%.3fs, I=%.3f)",
                       fz, err, ux, dt, g_int_acc);
}

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;

  // 目前先设为 false，直到 MoveIt 修正 QoS 设置以启用进程内通信（intra process comms）
  node_options.use_intra_process_comms(false);
  node_ = std::make_shared<rclcpp::Node>("servo_node", node_options);

  rclcpp::sleep_for(std::chrono::seconds(4)); // 等待 RViz 启动完成
  auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
  auto planning_scene_monitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(
      node_, "robot_description", tf_buffer, "planning_scene_monitor");

  // 确保 PSM 能从 /joint_states 实时更新机器人状态
  if (planning_scene_monitor->getPlanningScene())
  {
    planning_scene_monitor->startStateMonitor("/joint_states");
    planning_scene_monitor->setPlanningScenePublishingFrequency(25);
    planning_scene_monitor->startPublishingPlanningScene(
        planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE,
        "/moveit_servo/publish_planning_scene");
    planning_scene_monitor->startSceneMonitor();
    planning_scene_monitor->providePlanningSceneService();
  }
  else
  {
    RCLCPP_ERROR(LOGGER, "Planning scene 未正确配置");
    return EXIT_FAILURE;
  }

  twist_cmd_pub_ = node_->create_publisher<geometry_msgs::msg::TwistStamped>(
      "servo_node/delta_twist_cmds", 10);

  // ★ 新增：订阅仿真力传感器
  sim_force_sub_ = node_->create_subscription<geometry_msgs::msg::WrenchStamped>(
      "/sim_force",
      rclcpp::SensorDataQoS(),
      &simForceCallback);

  // 载入 Servo 参数并启动
  auto servo_parameters = moveit_servo::ServoParameters::makeServoParameters(node_);
  auto servo = std::make_unique<moveit_servo::Servo>(node_, servo_parameters, planning_scene_monitor);
  servo->start();

  // ★ 修改：定时器仍为50ms（20Hz）。若力控需要更快响应，可改为10ms（100Hz）。
  rclcpp::TimerBase::SharedPtr timer = node_->create_wall_timer(50ms, publishCommands);

  auto executor = std::make_unique<rclcpp::executors::MultiThreadedExecutor>();
  executor->add_node(node_);
  executor->spin();

  rclcpp::shutdown();
  return 0;
}
